//---------------------------------------------------------------------------


#pragma hdrstop

#include "stdafx.h"
#include "Computer.h"

//---------------------------------------------------------------------------

#pragma package(smart_init)

#define ADDINDEX_CRX(A, B) ((A + B) % COMP_RX_BUFF_SIZE)

unsigned char Comp_Receive[COMP_R_LENGTH] = {0};
unsigned char Comp_Send[COMP_S_LENGTH] = {0};
CompRXStruct CompRX = {0};

void Comp_Init(void)
{
	Comp_Receive_Init();
	Comp_Send_Init();
	CompRX_Init();
}

void Comp_Receive_Init(void)
{
	for(int i=0; i < COMP_R_LENGTH; i++)
	{
		Comp_Receive[i] = 0;
	}
}

void Comp_Send_Init(void)
{
	for(int i=0; i < COMP_S_LENGTH; i++)
	{
		Comp_Send[i] = 0;
	}
}

void CompRX_Init(void)
{
	for(int i=0; i < COMP_RX_BUFF_SIZE; i++)
	{
		CompRX.Buff[i] = 0;
	}
	CompRX.EndIndex = 0;
	CompRX.NowIndex = 0;
}

bool CompRX2Receive(void)
{
  int num_nondealed_data = 0;
	int num_read_byte = 0;
	unsigned char checksum;
	num_read_byte = COMP_PORT->Read(CommBuff, COMM_BUFF_SIZE);
	AnsiString str_show = "CompRX:";

	for(int i=0; i < num_read_byte; i++)
	{
		CompRX.Buff[CompRX.EndIndex] = CommBuff[i];
		CompRX.EndIndex = ADDINDEX_CRX(CompRX.EndIndex,1);

		#ifdef bShow_CompRX
			if(bShow_CompRX)
			{
        str_show = str_show + " " + IntToStr(CommBuff[i]);
			}
		#endif
	}
	#ifdef bShow_CompRX
		if(bShow_CompRX)
		{
			Memo_ShowInfo->Lines->Add(str_show);
		}
	#endif

	if (CompRX.NowIndex <= CompRX.EndIndex)
	{
		num_nondealed_data = CompRX.EndIndex - CompRX.NowIndex;
	}
	else
	{
		num_nondealed_data = COMP_RX_BUFF_SIZE - (CompRX.NowIndex - CompRX.EndIndex);
	}

	while(num_nondealed_data >= COMP_R_LENGTH)
	{
		if(CompRX.Buff[ADDINDEX_CRX(CompRX.NowIndex,COMP_R_HEAD1)] == 255)
		{
			checksum = 0;
			for(int i=COMP_R_MOTION_MODE; i<COMP_R_CHECKSUM; i++)
			{
				checksum += CompRX.Buff[ADDINDEX_CRX(CompRX.NowIndex,i)];
			}
			checksum = ~checksum;
			if(checksum == CompRX.Buff[ADDINDEX_CRX(CompRX.NowIndex,COMP_R_CHECKSUM)])
			{
				for(int i=COMP_R_HEAD1; i<COMP_R_LENGTH; i++)
				{
					Comp_Receive[i] = CompRX.Buff[ADDINDEX_CRX(CompRX.NowIndex,i)];
				}
				Receive2Motion();
				CompRX.NowIndex = 0;
				CompRX.EndIndex = 0;
				return true;
			}
			else
			{
				CompRX.NowIndex = ADDINDEX_CRX(CompRX.NowIndex,1);
			}
		}
		else
		{
			CompRX.NowIndex = ADDINDEX_CRX(CompRX.NowIndex,1);
		}

		if (CompRX.NowIndex <= CompRX.EndIndex)
		{
			num_nondealed_data = CompRX.EndIndex - CompRX.NowIndex;
		}
		else
		{
			num_nondealed_data = COMP_RX_BUFF_SIZE - (CompRX.NowIndex - CompRX.EndIndex);
		}
	}
	return false;
}

void Receive2Motion(void)
{
	if(	Comp_Receive[COMP_R_MOTION_MODE] != NO_MOTION &&
			Motion.Replaceable)
	{
		if(	Comp_Receive[COMP_R_MOTION_MODE] > 110 &&
				Comp_Receive[COMP_R_MOTION_MODE] <= (100+WALK_RAISEARM))
		{
			Motion.Mode_RX = Comp_Receive[COMP_R_MOTION_MODE] - 100;
			Motion.Replaceable = false;
		}
		else if(Comp_Receive[COMP_R_MOTION_MODE] <= WALK_RAISEARM)
		{
			Motion.Mode_RX = Comp_Receive[COMP_R_MOTION_MODE];
			Motion.Replaceable = true;
		}
		else
		{
			Motion.Mode_RX = NO_MOTION;
			Motion.Replaceable = true;
		}
		Motion.ContinueDistance_RX = (float)((int)(Comp_Receive[COMP_R_CONT_DIS_L] +
																							(Comp_Receive[COMP_R_CONT_DIS_H] << 8)) - 32768) / 100;
		Motion.ContinueAngle_RX = (float)((int)(Comp_Receive[COMP_R_CONT_ANGLE_L] +
																					 (Comp_Receive[COMP_R_CONT_ANGLE_H] << 8)) - 32768) / 100;
		#ifdef bShow_MotionModeRX
			if(bShow_MotionModeRX)
			{
				Memo_ShowInfo->Lines->Add("MotionModeRX: " + IntToStr(Motion.Mode_RX) +
																	", DisRX: " + FormatFloat("0.00", Motion.ContinueDistance_RX) +
																	", AngleRX:" + FormatFloat("0.00", Motion.ContinueAngle_RX));
			}
		#endif
	}

	if(Comp_Receive[COMP_R_HEAD_MODE] != NO_MOTION)
	{
		Head.Mode_RX = Comp_Receive[COMP_R_HEAD_MODE];
		Head.Pos[0][0] = Comp_Receive[COMP_R_MOTOR_0_L];
		Head.Pos[0][1] = Comp_Receive[COMP_R_MOTOR_0_H];
		Head.Pos[1][0] = Comp_Receive[COMP_R_MOTOR_1_L];
		Head.Pos[1][1] = Comp_Receive[COMP_R_MOTOR_1_H];
		#ifdef bShow_HeadModeRX
			if(bShow_HeadModeRX)
			{
				Memo_ShowInfo->Lines->Add("HeadModeRX: " + IntToStr(Head.Mode_RX) +
																	", Motor0RX: " + IntToStr(Head.Pos[0][0] + (Head.Pos[0][1] << 8)) +
																	", Motor1RX:" + IntToStr(Head.Pos[1][0] + (Head.Pos[1][1] << 8)));
			}

		#endif
	}
	#ifdef bShow_CompReceive
		if(bShow_CompReceive)
		{
			AnsiString str_show = "CompReceive:";
			for(int i = COMP_R_HEAD1; i < COMP_R_LENGTH; i++)
			{
				str_show = str_show + " " + IntToStr(Comp_Receive[i]);
			}
      Memo_ShowInfo->Lines->Add(str_show);
		}
	#endif
}

void CompSend(void)
{
	int i;
	int temp0, temp1, temp2, temp3, temp4;

	temp0 = MotorData.Read[0][READ_LOW_POSITION] + (MotorData.Read[0][READ_HIGH_POSITION] << 8) -
					MotorCorrection_Intrinsic.InUB[0];
	temp1 = MotorData.Read[1][READ_LOW_POSITION] + (MotorData.Read[1][READ_HIGH_POSITION] << 8) -
					MotorCorrection_Intrinsic.InUB[1];
	temp2 = MotorData.Read[2][READ_LOW_POSITION] + (MotorData.Read[2][READ_HIGH_POSITION] << 8) -
					MotorCorrection_Intrinsic.InUB[2];
	temp3 = MotorData.Read[3][READ_LOW_POSITION] + (MotorData.Read[3][READ_HIGH_POSITION] << 8) -
					MotorCorrection_Intrinsic.InUB[3];
	temp4 = MotorData.Read[4][READ_LOW_POSITION] + (MotorData.Read[4][READ_HIGH_POSITION] << 8) -
					MotorCorrection_Intrinsic.InUB[4];

	Comp_Send[COMP_S_HEAD1] = 0xFF;
	Comp_Send[COMP_S_HEAD2] = 0xFE;
	Comp_Send[COMP_S_DIP] = Board_Receive[BOARD_R_DIP];
	Comp_Send[COMP_S_MOTOR_0_L] = temp0 & 0x00FF;
	Comp_Send[COMP_S_MOTOR_0_H] = (temp0 & 0xFF00) >> 8;
	Comp_Send[COMP_S_MOTOR_1_L] = temp1 & 0x00FF;
	Comp_Send[COMP_S_MOTOR_1_H] = (temp1 & 0xFF00) >> 8;
	Comp_Send[COMP_S_MOTOR_2_L] = temp2 & 0x00FF;
	Comp_Send[COMP_S_MOTOR_2_H] = (temp2 & 0xFF00) >> 8;
	Comp_Send[COMP_S_MOTOR_3_L] = temp3 & 0x00FF;
	Comp_Send[COMP_S_MOTOR_3_H] = (temp3 & 0xFF00) >> 8;
	Comp_Send[COMP_S_MOTOR_4_L] = temp4 & 0x00FF;
	Comp_Send[COMP_S_MOTOR_4_H] = (temp4 & 0xFF00) >> 8;
	Comp_Send[COMP_S_MOTION_DONE] = Motion.Doing;


	Comp_Send[COMP_S_AZIMUTH_L] = Board_Receive[BOARD_R_AZIMUTH_L];
	Comp_Send[COMP_S_AZIMUTH_H] = Board_Receive[BOARD_R_AZIMUTH_H];

	Comp_Send[COMP_S_CHECKSUM] = 0;
	for(i=COMP_S_HEAD2+1; i < COMP_S_CHECKSUM; i++)
	{
		Comp_Send[COMP_S_CHECKSUM] += Comp_Send[i];
	}
	Comp_Send[COMP_S_CHECKSUM] = ~Comp_Send[COMP_S_CHECKSUM];

	Comm_Write(COMP_PORT, Comp_Send, COMP_S_LENGTH);

	#ifdef bShow_CompSend
		if(bShow_CompSend)
		{
      AnsiString str_show = "CompSend:";
			for(int i = COMP_S_HEAD1; i < COMP_S_LENGTH; i++)
			{
				str_show = str_show + " " + IntToStr(Comp_Send[i]);
			}
      Memo_ShowInfo->Lines->Add(str_show);
		}
	#endif
}

void Comp_StoreToShareM(void)
{
	unsigned short temp0, temp1, temp2, temp3, temp4;

	temp0 = MotorData.Read[0][READ_LOW_POSITION] + (MotorData.Read[0][READ_HIGH_POSITION] << 8) -
					MotorCorrection_Intrinsic.InUB[0];
	temp1 = MotorData.Read[1][READ_LOW_POSITION] + (MotorData.Read[1][READ_HIGH_POSITION] << 8) -
					MotorCorrection_Intrinsic.InUB[1];
	temp2 = MotorData.Read[2][READ_LOW_POSITION] + (MotorData.Read[2][READ_HIGH_POSITION] << 8) -
					MotorCorrection_Intrinsic.InUB[2];
	temp3 = MotorData.Read[3][READ_LOW_POSITION] + (MotorData.Read[3][READ_HIGH_POSITION] << 8) -
					MotorCorrection_Intrinsic.InUB[3];
	temp4 = MotorData.Read[4][READ_LOW_POSITION] + (MotorData.Read[4][READ_HIGH_POSITION] << 8) -
					MotorCorrection_Intrinsic.InUB[4];

	Mem_Info->Strategy.RxData.start1Code = 0xFF;
	Mem_Info->Strategy.RxData.start2Code = 0xFE;
	Mem_Info->Strategy.RxData.dip = Board_Receive[BOARD_R_DIP];
	Mem_Info->Strategy.RxData.tilePos = temp0;
	Mem_Info->Strategy.RxData.panPos = temp1;
	Mem_Info->Strategy.RxData.waistPosRoll = temp2;
	Mem_Info->Strategy.RxData.waistPosTilt = temp3;
	Mem_Info->Strategy.RxData.waistPosPan = temp4;
	Mem_Info->Strategy.RxData.bodyDoingCode = Motion.Doing;
	Mem_Info->Strategy.RxData.azimuth = Board_Receive[BOARD_R_AZIMUTH_L] +
																			(Board_Receive[BOARD_R_AZIMUTH_H] << 8);
}

void Comp_LoadFromShareM(void)
{
	if(	Mem_Info->Strategy.TxData.bodyMotionCode != NO_MOTION &&
			Motion.Replaceable)
	{
		if(	Mem_Info->Strategy.TxData.bodyMotionCode > 110 &&
				Mem_Info->Strategy.TxData.bodyMotionCode <= (100+WALK_RAISEARM))
		{
			Motion.Mode_RX = Mem_Info->Strategy.TxData.bodyMotionCode - 100;
			Motion.Replaceable = false;
		}
		else if(Mem_Info->Strategy.TxData.bodyMotionCode <= WALK_RAISEARM)
		{
			Motion.Mode_RX = Mem_Info->Strategy.TxData.bodyMotionCode;
			Motion.Replaceable = true;
		}
		else
		{
			Motion.Mode_RX = NO_MOTION;
			Motion.Replaceable = true;
		}
		Motion.ContinueDistance_RX = Mem_Info->Strategy.TxData.dis;
		Motion.ContinueAngle_RX = Mem_Info->Strategy.TxData.angle;
		#ifdef bShow_MotionModeRX
			if(bShow_MotionModeRX)
			{
				Memo_ShowInfo->Lines->Add("MotionModeRX: " + IntToStr(Motion.Mode_RX) +
																	", DisRX: " + FormatFloat("0.00", Motion.ContinueDistance_RX) +
																	", AngleRX:" + FormatFloat("0.00", Motion.ContinueAngle_RX));
			}
		#endif
	}

	if(Mem_Info->Strategy.TxData.headMotionCode != NO_MOTION)
	{
		Head.Mode_RX = Mem_Info->Strategy.TxData.headMotionCode;
		Head.Pos[0][0] = Mem_Info->Strategy.TxData.tiltPos & 0x00FF;
		Head.Pos[0][1] = (Mem_Info->Strategy.TxData.tiltPos & 0xFF00) >> 8;
		Head.Pos[1][0] = Mem_Info->Strategy.TxData.panPos & 0x00FF;
		Head.Pos[1][1] = (Mem_Info->Strategy.TxData.panPos & 0xFF00) >> 8;
		#ifdef bShow_HeadModeRX
			if(bShow_HeadModeRX)
			{
				Memo_ShowInfo->Lines->Add("HeadModeRX: " + IntToStr(Head.Mode_RX) +
																	", Motor0RX: " + IntToStr(Head.Pos[0][0] + (Head.Pos[0][1] << 8)) +
																	", Motor1RX:" + IntToStr(Head.Pos[1][0] + (Head.Pos[1][1] << 8)));
			}

		#endif
	}
}
